#include "robot_manager.h"


void RobotManager::initializeMotors()
{
    // std::vector<double> mit_kp = {100000.0, 100000.0, 8000.0, 8000.0, 40.0, 40.0, 40.0};
    // std::vector<double> mit_kd = {50000.0, 50000.0, 800.0, 800.0, 4.0, 4.0, 4.0};

    mit_kp_.resize(robot_dof_);
    mit_kd_.resize(robot_dof_);
    mit_kp_ = {8000.0, 15000.0, 8000.0, 8000.0, 20.0, 20.0, 20.0};
    mit_kd_ = {800.0, 5000.0, 800.0, 800.0, 1.0, 1.0, 1.0};


    // std::vector<double> collision_force_threshold = {5.0, 10.0, 3.0, 5.0, 1.0, 1.0, 1.0};

    // 设置参数

    // this->declare_parameter("mit_kp", mit_kp);
    // this->declare_parameter("mit_kd", mit_kd);
    // this->declare_parameter("collision_force_threshold", collision_force_threshold);


    //! 添加电机，根据提供的示例代码
    motors_.push_back(damiao::Motor(damiao::DM8009, 0x01, 0x11));
    motors_.push_back(damiao::Motor(damiao::DM8009, 0x02, 0x12));
    motors_.push_back(damiao::Motor(damiao::DM4340, 0x03, 0x13));
    motors_.push_back(damiao::Motor(damiao::DM4340, 0x04, 0x14));
    motors_.push_back(damiao::Motor(damiao::DM4310, 0x05, 0x15));
    motors_.push_back(damiao::Motor(damiao::DM4310, 0x06, 0x16));
    motors_.push_back(damiao::Motor(damiao::DM4310, 0x07, 0x17));
    gripper_ = damiao::Motor(damiao::DM4310, 0x08, 0x18);

    dm_ = std::make_shared<damiao::Motor_Control>();
    //! 添加电机到控制器
    for (auto &motor: motors_)
    {
        dm_->addMotor(&motor);
        //! TODO: disable是否可以忽略？
        // dm_->disable(motor);
    }
    dm_->addMotor(&gripper_);

    //! 切换到位置速度模式并使能电机
    for (auto &motor: motors_)
    {
        // if (dm_->switchControlMode(motor, damiao::POS_VEL_MODE))
        if (dm_->switchControlMode(motor, damiao::MIT_MODE))
            ZLOGI("Switch motor %d to MIT_MODE Success", motor.GetSlaveId());
        dm_->enable(motor);
    }
    if (dm_->switchControlMode(gripper_, damiao::MIT_MODE))
        ZLOGI("Switch motor 8(gripper) to MIT_MODE Success", gripper_.GetSlaveId());
    dm_->enable(gripper_);
}